import cv2
import open3d as o3d
import copy

def load_camera_param(stereo_camera_param_path: str):
    stereo_camera_param_cvfile = cv2.FileStorage(stereo_camera_param_path, cv2.FILE_STORAGE_READ)
    cameraMatrix = stereo_camera_param_cvfile.getNode("cameraMatrix").mat()
    distCoeffs = stereo_camera_param_cvfile.getNode("distCoeffs").mat()


if __name__ == "__main__":
    stereo_camera_param_path = "D:/VSCodeProjects/calibration/weihai/CalibrateStereoCam_result.yml"
    stereo_camera_param_cvfile = cv2.FileStorage(stereo_camera_param_path, cv2.FILE_STORAGE_READ)
    R = stereo_camera_param_cvfile.getNode("R").mat()
    T = stereo_camera_param_cvfile.getNode("T").mat()
    eyetohand = stereo_camera_param_cvfile.getNode("EtoHandY").mat()
    print(R)
    print(T)
    print(eyetohand)

    # 左相机坐标系
    cam_left = o3d.geometry.TriangleMesh.create_coordinate_frame(size=30, origin=[0, 0, 0])

    # 右相机坐标系
    cam_right = copy.deepcopy(cam_left)
    cam_right.rotate(R, center=(0, 0, 0))
    cam_right.translate(T)

    # 纱管手
    hand_y = copy.deepcopy(cam_left)
    hand_R = eyetohand[:3,:3]
    hand_t = eyetohand[:3, 3]
    print(hand_R)
    print(hand_t)
    hand_y.rotate(hand_R, center=(0, 0, 0))
    hand_y.translate(hand_t)

    o3d.visualization.draw_geometries([cam_left, cam_right, hand_y])

    # print(eyetohand)